/*
 * @Author: Meroke
 * @Date: 2022-04-02 15:36:47
 * @LastEditors: Meroke
 * @LastEditTime: 2022-04-07 20:14:14
 * @Description: base of servo
 * @FilePath: \MDK-ARMe:\Program\Code\cpp\STM32\project\arm_test\Servo\Inc\servo_mode.h
 */
#ifndef __SERVO_MODE__
#define __SERVO_MODE__
#include "tim.h"
#include "stdlib.h"
#include "stm32f4xx_hal.h"
#include <stdio.h>

enum MODE
{
    STOP=0,
    RUN=1
};
enum ACTION_MODE
{
    HOMING=0,
    GRAB,
    SWING
};
typedef struct servo Servo_Control;
struct servo
{
    /*
    * base attribute of servo
    * init in function [_servo_init -> servo_init]
    */
    uint8_t _servo_len; // servo number.
    /*
        (left)[2] forearm angle 0~70 (Structural constraints)
        (right)[1] upperarm angle 0~90 (Structural constraints)
        (base)[0] rotary angle 0~180 (servo constraints)        
    */
    uint8_t *_target_angle; // servo will move to the target angle
    uint8_t *_current_angle; // servo's current angle
    uint32_t *_channel_group; // TIM channels -> servos, default 1,2,3
    TIM_HandleTypeDef * _htim;  // TIM, default htim5
    uint8_t _stop_flag; 
    /*
    * function pointer
    * for quickly adjust action mode
    */
    uint8_t (*_select_mode)(Servo_Control *sc);
    uint8_t (*_servo_move)(Servo_Control *sc);

    /*
    * init pointer 
    * and decide the method of driving servo
    */
    uint8_t (*_servo_init)(Servo_Control *sc, TIM_HandleTypeDef * htim,uint32_t channel_group[],uint8_t angle[],uint8_t servo_len);
    uint8_t (*_servo_set_angle)(Servo_Control *sc, uint8_t angle[]);
};

// Initialization Function Entry
uint8_t Register_Init(Servo_Control *sc);

uint8_t servo_init(Servo_Control *sc,TIM_HandleTypeDef * htim,uint32_t channel_group[],uint8_t angle[],uint8_t servo_len);
uint8_t servo_set_angle(Servo_Control *sc,uint8_t angle[]);

// TEST move function. Standard function are [drab],[swing]...
// _servo_move -> drab/swing...
uint8_t servo_move(Servo_Control *sc);
uint8_t servo_move_group(Servo_Control *sc);
uint8_t homing(Servo_Control *sc);
uint8_t grab(Servo_Control *sc);
uint8_t swing(Servo_Control *sc);
 
// Modes of Driving: PMW ...
void PWM_driving_servo(TIM_HandleTypeDef * htim,uint32_t Channel,uint8_t angle);

// Extra Control Mode
// Absolute Angle Control
uint8_t force_move_angle(Servo_Control *sc,uint8_t target_angle);

uint8_t select_mode(Servo_Control *sc, uint8_t mode);


#endif
